"""
Create: JiBingyu 2024-07-15
Description: 该文件是算法的主文件，用于调用算法库中的算法实现，实现多艇编队控制。
Update:
1.增加了绘图功能，可以实时显示任务点、障碍物和USV的位置。
2.障碍物的显示和说明书上的不一样，说明书上半径为200速度为0，代码中半径为0速度为200。（如果使用200的半径，USV最开始就生成在障碍物内部）
4.环境没有对USV速度和角度的限制，需要根据比赛说明限制。
5.参考点和参考位置并不是每一时刻都可以获取，使用全局变量传递
6.对四大类信息【参考点，参考位置，障碍物，舰船信息】进行了数据处理
0725
1.速度的设定没有限制，但是随着输入速度增大（1000左右），实际速度会有一定的下降
2.每次控制仿真的执行时间大概为0.1s，因此每次运动距离为0.1*速度
3.Yaw的最大变化角度是1度
4.Vel的变化倍数是输入变化的0.017986297607421875倍（当前0，输入100，实际变为1.7986145）
0727
1. 就算使用新线程也会减慢主线程的速度，因此需要考虑如何加快速度【*】
2. 不考虑碰撞，完成航点编队任务【没完成无具体参考点的任务，因此需要考虑在无参考目标下如何分配任务】
3. 控制思路
    1、人工势场：【先写一版】
        1）每次决策的时候根据飞机的位置和障碍物的位置计算势场，引力就是最终的目标位置（是否需要考虑计算其他最优航点）
        2）分配引力斥力的比例大小
    2、强化学习
        1）决策时间相同，由于障碍物、飞机数目等环境信息每次都变化，需要考虑如何处理不同大小的输入
        2）网络收敛时间
        3）训练的目标，如何减弱奖励稀疏的环境
"""

import ctypes
import math
import os
import shutil
import threading
import time
from typing import List

import matplotlib.patches as patches
import matplotlib.pyplot as plt
import numpy as np

from Function_j import *
from MultiUSVModel import MultiUSVModel
from StructType import *

# 全局变量定义
Task_data = []  # 任务点， taskPath_list并不是每次都有，所以需要一个全局变量来存储
Refe_data = []  # 参考点， referInfo_list并不是每次都有，所以需要一个全局变量来存储
Obs_data = []  # 障碍物，其中obsR和fOBSV与说明书上相反，代码中半径为0速度为200，说明书上半径为200速度为0
USV_data = []  # 实时的USV位置信息
targer_location = []  # 目标位置
plotting = True  # 是否绘图
# 各艇控制指令
multiUSVInitInfo: List[USVInitStructType] = []

time_time = 0  # 记录运行时间，存放在Time.txt中


# 清空dataRecord文件夹
def clear_dataRecord_folder():
    folder_path = "dataRecord"  # 指定 dataRecord 文件夹的路径
    for filename in os.listdir(folder_path):
        file_path = os.path.join(folder_path, filename)
        try:
            if os.path.isfile(file_path) or os.path.islink(file_path):
                os.unlink(file_path)
            elif os.path.isdir(file_path):
                shutil.rmtree(file_path)
        except Exception as e:
            print(f"Failed to delete {file_path}. Reason: {e}")


clear_dataRecord_folder()


def algorithmImpl(
    algIterNum,
    taskPath_data,  # 编队航线
    taskPath_size,
    referInfo_data,  # 编队构型
    referInfo_size,
    curOBSFusion_data,  # 障碍物信息
    curOBSFusion_size,
    multiUSVStatus_data,  # 编队内艇信息
    multiUSVStatus_size,
):
    global Task_data
    global Refe_data
    global Obs_data
    global USV_data
    global targer_location

    multiUSVStatus_list = Get_USV_data(
        taskPath_data,
        taskPath_size,
        referInfo_data,
        referInfo_size,
        curOBSFusion_data,
        curOBSFusion_size,
        multiUSVStatus_data,
        multiUSVStatus_size,
    )

    #################################### 航点优化部分 ###############################

    # 初始化目标位置
    if len(targer_location) == 0:
        targer_location = np.zeros((2, len(Refe_data), 2))
    # 参考ID和参考点的XY坐标,选择主舰的航点
    refer_ID = [info[2] for info in Refe_data if info[-1] == 1]
    refer_location = [
        Go_to_xy2(
            multiUSVStatus_list[indx].dbCurX,
            multiUSVStatus_list[indx].dbCurY,
            Task_data[-1][0],
            Task_data[-1][1],
        )
        for indx, info in enumerate(multiUSVStatus_list)
        if info.iUSVId in refer_ID
    ]

    ##################################################################################
    # TODO: 在此处添加相关控制逻辑代码
    # 根据实时位置，任务信息求下一时刻艇的期望航速航向
    multiUSVInitInfo: List[USVInitStructType] = []
    for i, it in enumerate(multiUSVStatus_list):
        updateUSVControl = USVInitStructType()
        updateUSVControl.iUSVId = it.iUSVId
        updateUSVControl.dbInitX = it.dbCurX
        updateUSVControl.dbInitY = it.dbCurY

        #################################### 控制算法部分 ###############################

        indices = [
            index for index, info in enumerate(Refe_data) if info[2] == it.iUSVId
        ]
        if len(indices):  # 检查是否至少有一个匹配
            targer_location[0][i] = Go_to_va(refer_location[0], Refe_data, indices)
            # 检查潜在位置是否发生碰撞
            if is_collision_free(
                targer_location[0][i][0],
                targer_location[0][i][1],
                multiUSVStatus_list[:i] + multiUSVStatus_list[i + 1 :],
            ):
                targer_location[1][i] = targer_location[0][i]
            else:
                # 如果发生碰撞，调整目标位置
                adjusted_location = None
                for len_di in range(100, 1000, 10):  # 尝试不同角度的偏移
                    new_x = targer_location[0][i][0] + len_di * np.cos(90 - it.fCurYaw)
                    new_y = targer_location[0][i][1] + len_di * np.sin(90 - it.fCurYaw)
                    if is_collision_free(
                        new_x,
                        new_y,
                        multiUSVStatus_list[:i] + multiUSVStatus_list[i + 1 :],
                    ):
                        adjusted_location = [new_x, new_y]
                        break
                if adjusted_location:
                    targer_location[1][i] = adjusted_location
                else:
                    # 如果无法找到合适的调整位置，保持原地
                    targer_location[1][i] = [it.dbCurX, it.dbCurY]
            # targer_location[i] = Go_to_va(refer_location[0], Refe_data, indices)
            updateUSVControl.fInitVel, updateUSVControl.fInitYaw = Go_to_xy(
                it.dbCurX,
                it.dbCurY,
                targer_location[1][i][0],
                targer_location[1][i][1],
                [3, 15],
            )

        ########################################## End #################################
        else:
            # 处理没有找到匹配的情况
            print("No matching USVId found.")

        multiUSVInitInfo.append(updateUSVControl)
    return multiUSVInitInfo
    ##################################################################################


def Get_USV_data(
    taskPath_data,  # 编队航线
    taskPath_size,
    referInfo_data,  # 编队构型
    referInfo_size,
    curOBSFusion_data,  # 障碍物信息
    curOBSFusion_size,
    multiUSVStatus_data,  # 编队内艇信息
    multiUSVStatus_size,
):
    global Task_data
    global Refe_data
    global Obs_data
    global USV_data
    taskPath_list: List[SFormationTaskInfo] = [
        taskPath_data[index] for index in range(taskPath_size.value)
    ]
    referInfo_list: List[referUSVInfo] = [
        referInfo_data[index] for index in range(referInfo_size.value)
    ]
    curOBSFusion_list: List[InputOBSStruct] = [
        curOBSFusion_data[index] for index in range(curOBSFusion_size.value)
    ]
    multiUSVStatus_list: List[USVOutPutStructType] = [
        multiUSVStatus_data[index] for index in range(multiUSVStatus_size.value)
    ]
    Task_data = [] if len(taskPath_list) != 0 else Task_data
    Refe_data = [] if len(referInfo_list) != 0 else Refe_data
    Obs_data = [] if len(curOBSFusion_list) != 0 else Obs_data
    USV_data = [] if len(multiUSVStatus_list) != 0 else USV_data
    for it in taskPath_list:
        Task_data.append([it.referX, it.referY])
    for it in referInfo_list:
        Refe_data.append([it.relatDis, it.relatYaw, it.USVID, it.type])
    for it in curOBSFusion_list:
        # 这里的 obsR 和 fOBSV与说明书上反的
        Obs_data.append([it.dOBSX, it.dOBSY, it.fOBSV])
    for it in multiUSVStatus_list:
        USV_data.append([it.dbCurX, it.dbCurY])

    return multiUSVStatus_list


def plot_graph():
    global plotting, Task_data, Obs_data, USV_data, multiUSVInitInfo, targer_location
    plt.ion()  # 开启交互模式
    fig, ax = plt.subplots()
    task_scatter = ax.scatter([], [], color="blue")
    obs_scatter = ax.scatter([], [], color="green")
    usv_scatter = ax.scatter([], [], color="red")
    target_scatter = ax.scatter([], [], color="orange")
    arrows = []
    while plotting:
        # 更新数据
        task_scatter.set_offsets(Task_data)
        obs_scatter.set_offsets(Obs_data)
        usv_scatter.set_offsets(USV_data)
        target_scatter.set_offsets(targer_location[1])
        # 目标点
        # plt.scatter([x for x, _ in Task_data], [y for _, y in Task_data], color="blue")
        # for i, (x, y) in enumerate(Task_data):
        #     plt.text(x, y, f"Task{i}", color="blue", fontsize=8)
        #     circle = patches.Circle((x, y), 50, fill=False, edgecolor="blue")
        #     plt.gca().add_patch(circle)

        # 障碍物
        # plt.scatter(
        #     [x for x, _, _ in Obs_data], [y for _, y, _ in Obs_data], color="green"
        # )
        # for i, (x, y, r) in enumerate(Obs_data):
        #     plt.text(x, y, f"Obs{i+1}", color="green", fontsize=8)
        #     circle = patches.Circle((x, y), r, fill=False, edgecolor="green")
        #     plt.gca().add_patch(circle)

        # 更新箭头
        for arrow in arrows:
            arrow.remove()
        arrows.clear()
        for updateUSVControl in multiUSVInitInfo:
            dx = updateUSVControl.fInitVel * np.cos(
                np.radians(90 - updateUSVControl.fInitYaw)
            )
            dy = updateUSVControl.fInitVel * np.sin(
                np.radians(90 - updateUSVControl.fInitYaw)
            )
            arrow = ax.arrow(
                updateUSVControl.dbInitX,
                updateUSVControl.dbInitY,
                dx,
                dy,
                head_width=10,
                head_length=20,
                fc="blue",
                ec="blue",
            )
            arrows.append(arrow)

        plt.draw()
        plt.pause(0.2)  # 调整绘图更新频率
    plt.ioff()  # 关闭交互模式


if __name__ == "__main__":
    os.add_dll_directory(os.path.join(os.getcwd(), "./dll"))
    cmultilUSV = MultiUSVModel(
        dll_path=r"./dll/usvmodel.dll",
        dll_deps=[os.path.join(r"./dll", dll_i) for dll_i in os.listdir(r"./dll")],
    )
    # 创建并启动绘图线程
    plot_thread = threading.Thread(target=plot_graph)
    plot_thread.start()

    # 态势信息
    curOBSFusion_data = ctypes.POINTER(InputOBSStruct)()  # 指向InputOBSStruct类型的指针
    curOBSFusion_size = ctypes.c_int()  # 障碍物数量
    # 导航信息
    multiUSVStatus_data = ctypes.POINTER(USVOutPutStructType)()
    multiUSVStatus_size = ctypes.c_int()

    isEnd_int = ctypes.c_int()
    # 任务航路
    taskPath_data = ctypes.POINTER(SFormationTaskInfo)()
    taskPath_size = ctypes.c_int()
    # 构型信息
    referInfo_data = ctypes.POINTER(referUSVInfo)()
    referInfo_size = ctypes.c_int()
    # 整个任务迭代次数
    iterNum = 0
    # 控制模式，1 - 航速航向；2 - 航路，建议使用航速航向
    flagModel = 1
    # 算法更新后清零
    algIterNum = 0

    # 初始化
    cmultilUSV.multiInitialize(flagModel)
    # time_time = time.time()
    # # 生成Time.txt文件
    # with open("Time.txt", "w") as f:
    #     f.write(f"Time: {time_time}\n")

    try:
        for i in range(1000 * 20):
            # # 记录每次仿真时间
            # with open("Time.txt", "a") as f:
            #     f.write(f"{time.time() - time_time}\n")
            # 更新控制指令
            cmultilUSV.updateMultiUSVControl(
                (USVInitStructType * len(multiUSVInitInfo))(*multiUSVInitInfo),
                len(multiUSVInitInfo),
                iterNum,
                flagModel,
            )
            # 获取导航信息和态势信息
            cmultilUSV.getMultiNaviObsInfo(
                iterNum,
                ctypes.byref(multiUSVStatus_data),
                ctypes.byref(multiUSVStatus_size),
                ctypes.byref(curOBSFusion_data),
                ctypes.byref(curOBSFusion_size),
            )
            # 获取任务信息
            rcvFlag = cmultilUSV.getTaskInfo(
                ctypes.byref(isEnd_int),
                ctypes.byref(taskPath_data),
                ctypes.byref(taskPath_size),
                ctypes.byref(referInfo_data),
                ctypes.byref(referInfo_size),
                iterNum,
            )
            # 触发任务
            if 1 == rcvFlag:
                if 1 == isEnd_int.value:  # 到达最后一个任务点，不再发送任务
                    cmultilUSV.taskEnd(iterNum, flagModel)
                    break  # 跳出算法循环
                algIterNum = 0  # 新任务，次数清零
            # 算法更新控制指令
            multiUSVInitInfo: List[USVInitStructType] = algorithmImpl(
                algIterNum,
                taskPath_data,
                taskPath_size,
                referInfo_data,
                referInfo_size,
                curOBSFusion_data,
                curOBSFusion_size,
                multiUSVStatus_data,
                multiUSVStatus_size,
            )

            # 迭代次数更新
            iterNum += 1
            algIterNum += 1
        # 当主线程的任务完成后，更新标志以停止绘图线程的循环
    except KeyboardInterrupt:
        plotting = False
        # 等待绘图线程结束
        plot_thread.join()
